//************************************************************
//    > File Name: ottdhcp.c
//    > Author: jiayue
//    > Mail: tjyemail@sina.com 
//    > Created Time: 2018年06月26日 星期二 11时38分04秒
//***********************************************************

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <unistd.h>
//#include <asm/types.h>
#include <arpa/inet.h>
#include <linux/if.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <linux/kernel.h>
#include <linux/netlink.h>
#include <linux/rtnetlink.h>
#include <linux/types.h>
//#include <net/if.h>
//#include <netinet/ether.h>
//#include <netinet/in.h>
#include <sys/ioctl.h>
#include <sys/wait.h>
#include <sys/socket.h>
#include <sys/types.h>


#define OTT_BUF_SIZE 1024
#define OTT_IF_NAME "eth0"
#define OTT_ETH_TYPE 0x7777
#define OTT_REQ_TIMEOUT 5
#define OTT_SOCK_TIMEOUT 1

// 0:debug off, 1:console, 2:stdout
int OTT_DEBUG_TYPE=1;
#define OTT_DEBUG(fmt, argv...) do { \
			if (OTT_DEBUG_TYPE == 1){ \
				FILE *__ott_fp = fopen("/dev/console","a+"); \
				if (__ott_fp){ \
					fprintf(__ott_fp, fmt, ## argv); \
					fclose(__ott_fp); \
				} \
			} \
			if (OTT_DEBUG_TYPE == 2) printf(fmt, ## argv); \
}while(0)

int ott_sock_rawfd = -1;			// send/recv socket fd
int ott_sock_netlink = -1;			// netlink to get ethernet up/down event
uint8_t ott_self_mac[6];			// ott self mac
struct sockaddr_ll ott_iface_addr;	// ott interface address

int ott_bridge_mode = 0;			// 1:bridge, 0:route
uint8_t ott_gateway_mac[6];
char ott_gateway_time[32];			// YYYYMMDD.hhmmss
char ott_iface_ip[OTT_BUF_SIZE];
char ott_iface_gw[OTT_BUF_SIZE];
char ott_iface_mask[OTT_BUF_SIZE];

void destroy(int flag)
{
	// deal ctrl+c
	if (ott_sock_rawfd!=-1) close(ott_sock_rawfd);
	if (ott_sock_netlink!=-1) close(ott_sock_netlink);
	OTT_DEBUG("...user cancel test, clean ok...\n");
	exit(0);
}

int dhcp_dump_raw(char *rawbuf)
{
	if (rawbuf == NULL) return 0;
	uint8_t *hexbuf = (uint8_t*)rawbuf;
	// packet data length, big endian
	uint16_t len = (hexbuf[14]<<8) | hexbuf[15];
	// ether frame header
	OTT_DEBUG("DUMP ETHER HEADER :\n");
	OTT_DEBUG(" 6  | %02X %02X %02X %02X %02X %02X\n",
			hexbuf[0],hexbuf[1],hexbuf[2],
			hexbuf[3],hexbuf[4],hexbuf[5]);
	OTT_DEBUG(" 6  | %02X %02X %02X %02X %02X %02X\n",
			hexbuf[6],hexbuf[7],hexbuf[8],
			hexbuf[9],hexbuf[10],hexbuf[11]);
	OTT_DEBUG(" 2  | 0x%02X%02X  ETHER_TYPE\n",
			hexbuf[12],hexbuf[13]);
	OTT_DEBUG(" 2  | 0x%02X%02X  LEN  %d\n",
			hexbuf[14],hexbuf[15],len);
	OTT_DEBUG(" 2  | 0x%02X TYPE  0x%02X CMD\n",
			hexbuf[16],hexbuf[17]);
	if (len<1) return 0;
	// packet data
	OTT_DEBUG("DUMP PACKET DATA :\n");
	char *pkdata = rawbuf+18;
	OTT_DEBUG("% 3d | %s\n",len,pkdata);
	return 0;
}

int dhcp_init_rawfd(char *ifname, uint8_t *ottmac, struct sockaddr_ll *addr)
{
	if (!ifname || !ottmac || !addr) return -1;
	int fd = socket(PF_PACKET,SOCK_RAW,htons(ETH_P_ALL));
	if (fd<0){
		OTT_DEBUG("open raw socket error\n");
		return -1;
	}

	// set socket timeout
	struct timeval timeout = {OTT_SOCK_TIMEOUT,0};
	setsockopt(fd,SOL_SOCKET,SO_RCVTIMEO,(char*)&timeout,sizeof(struct timeval));

	// ott mac: ifmac.ifr_hwaddr.sa_data, 6 bytes
	struct ifreq ifmac;
	memset(&ifmac,0,sizeof(struct ifreq));
	strncpy(ifmac.ifr_name,ifname,IFNAMSIZ-1);
	if (ioctl(fd,SIOCGIFHWADDR,&ifmac)<0){
		OTT_DEBUG("can't find interface %s mac\n",ifname);
		return -1;
	}
	memcpy(ottmac,ifmac.ifr_hwaddr.sa_data,6);
	
	// sockaddr_ll
	memset(addr,0,sizeof(*addr));
	addr->sll_family = AF_PACKET;
	addr->sll_protocol = htons(ETH_P_ALL);
	//addr->sll_pkttype = PACKET_BROADCAST;
	addr->sll_ifindex = if_nametoindex(ifname);
	//addr->sll_ifindex = ifidx->ifr_ifindex;

	if (bind(fd,(struct sockaddr*)addr,sizeof(*addr))<0){
		OTT_DEBUG("bind socket error\n");
		close(fd);
		return -1;
	}
	return fd;
}

size_t dhcp_build_rawbuf(char *rawbuf, size_t bufsize,
		uint8_t srcmac[], uint8_t destmac[],
		uint8_t type, uint8_t cmd,
		char *pkdata, uint16_t pklen)
{
	if (!rawbuf || bufsize-18U<pklen) return 0;
	size_t buflen = 0;
	memset(rawbuf,0,bufsize);
	
	// ether header [6+6+2]
	memcpy(rawbuf,destmac,6);
	memcpy(rawbuf+6,srcmac,6);
	uint16_t etype = htons(OTT_ETH_TYPE);
	//uint16_t etype = htons(0x0800);
	memcpy(rawbuf+12,&etype,2);
	
	buflen = 6+6+2;
	// packet data length, big endian
	rawbuf[buflen++] = (pklen>>8) & 0xFF;
	rawbuf[buflen++] = pklen & 0xFF;
	rawbuf[buflen++] = type;
	rawbuf[buflen++] = cmd;
	// packet data
	if (pkdata!=NULL && pklen!=0) memcpy(rawbuf+buflen,pkdata,pklen);
	buflen += pklen;

	return buflen;
}

size_t dhcp_sendto(int sockfd, char *ifname, char *rawbuf, size_t buflen)
{
	if (sockfd<1 || !ifname || !rawbuf || buflen<1) return 0;
	// build socket address
	struct sockaddr_ll addr;
	memset(&addr,0,sizeof(addr));
	addr.sll_family = AF_PACKET;
	addr.sll_ifindex = if_nametoindex(ifname);
	memcpy(addr.sll_addr,rawbuf,6);

	//size_t ret = sendto(sockfd,rawbuf,buflen,0,(struct sockaddr*)&addr,sizeof(addr));
	size_t ret = sendto(sockfd,rawbuf,buflen,0,
			(struct sockaddr*)&ott_iface_addr,sizeof(ott_iface_addr));
	//OTT_DEBUG("rawbuf bytes %d, sendto bytes %d\n",buflen,ret);
	return ret;
}

size_t dhcp_recvfrom(int sockfd, char *ifname, char *rawbuf, size_t bufsize)
{
	if (sockfd<1 || !ifname || !rawbuf || bufsize<18) return 0;
	memset(rawbuf,0,bufsize);
	// build sockaddr
	struct sockaddr_ll addr;
	memset(&addr,0,sizeof(addr));
	addr.sll_family = AF_PACKET;
	addr.sll_ifindex = if_nametoindex(ifname);
	addr.sll_protocol = htons(ETH_P_ALL);

	socklen_t addrlen = sizeof(struct sockaddr_ll);
	//size_t buflen = recvfrom(sockfd,rawbuf,bufsize,0,(struct sockaddr*)&addr,&addrlen);
	size_t buflen = recvfrom(sockfd,rawbuf,bufsize,0,
			(struct sockaddr*)&ott_iface_addr,&addrlen);
	//OTT_DEBUG("recvfrom %d bytes\n",pklen);
	if (buflen<bufsize) rawbuf[buflen] = '\0';
	return buflen;
}

int dhcp_mac_equal(uint8_t mac1[], uint8_t mac2[], size_t size)
{
	if (mac1==mac2) return 1;
	if (!mac1 || !mac2) return 0;
	while(size--)
		if (mac1[size]!=mac2[size]) 
			return 0;
	return 1;
}

int dhcp_check_packet(char *rawbuf, size_t buflen, uint8_t cmd)
{
	if (!rawbuf) return 0;
	uint8_t *recvmac = (uint8_t*)rawbuf;
	if (!dhcp_mac_equal(ott_self_mac,recvmac,6)) return 0;
	int offset = 18;		// mac6+6, ether_type 2, len 2, type 1, cmd 1
	if ((int)buflen < offset) return 0;
	uint16_t ethertype = (rawbuf[12]<<8) | (rawbuf[13]);
	if (ethertype != OTT_ETH_TYPE) return 0;
	uint8_t type = (uint8_t) rawbuf[16];
	if (type != 1) return 0;
	if ((uint8_t)rawbuf[17] != cmd) return 0;

	return 1;
}

int dhcp_get_value(char *pkdata, char *key, char split, char *value)
{
	// dhcp_get_value(pkdata,"ottip=",',',ott_iface_ip);
	if (!pkdata || !key || !value) return 0;
	pkdata = strstr(pkdata,key);
	if (!pkdata) return 0;
	pkdata += strlen(key);
	char *end = strchr(pkdata,split);
	strncpy(value,pkdata,end?(size_t)(end-pkdata):strlen(pkdata));
	//memcpy(value,pkdata,end?(size_t)(end-pkdata):strlen(pkdata));
	return strlen(value)>0;
}

int dhcp_parser_packet(char *rawbuf, size_t buflen)
{
	if (!rawbuf || (int)buflen<18) return -1;

	uint8_t cmd = (uint8_t) rawbuf[17];
	// get gateway mac
	memset(ott_gateway_mac,0,6);
	memcpy(ott_gateway_mac,rawbuf+6,6);
	// data length, big endian
	uint16_t len = (rawbuf[14]<<8) | (rawbuf[15]);
	if (len<1) return 0;
	rawbuf += 18;
	rawbuf[len] = '\0';

	switch(cmd){
		case 0 : // discover gateway
			OTT_DEBUG("discover gateway success\n");
			break;
		case 1 : // check ip mode, '0':route, '1':bridge
			if (rawbuf[0] == '0') ott_bridge_mode = 0;
			if (rawbuf[0] == '1') ott_bridge_mode = 1;
			OTT_DEBUG("ip mode %s\n",ott_bridge_mode?"bridge":"route");
			break;
		case 2 : // request ip
			//printf("parser pkdata len %d\n%s\n",len,rawbuf);
			OTT_DEBUG("request ip success\n");
			memset(ott_iface_ip,0,sizeof(ott_iface_ip));
			memset(ott_iface_gw,0,sizeof(ott_iface_gw));
			memset(ott_iface_mask,0,sizeof(ott_iface_mask));
			dhcp_get_value(rawbuf,"ottip=",',',ott_iface_ip);
			dhcp_get_value(rawbuf,"gw=",',',ott_iface_gw);
			dhcp_get_value(rawbuf,"netmask=",',',ott_iface_mask);
			break;
		case 3 : // get gateway datetime
			OTT_DEBUG("get gateway datetime\n");
			memset(ott_gateway_time,0,sizeof(ott_gateway_time));
			memcpy(ott_gateway_time,rawbuf,8);
			ott_gateway_time[8] = '.';
			memcpy(ott_gateway_time+9,rawbuf+8,6);
			break;
	}
	return cmd;
}

int dhcp_set_network(char *ifname)
{
	if (!ifname) return 0;
	char cmd[OTT_BUF_SIZE];
	memset(cmd,0,OTT_BUF_SIZE);
	sprintf(cmd,"ifconfig %s:1 %s netmask %s",ifname,ott_iface_ip,ott_iface_mask);
	OTT_DEBUG("shell # %s\n",cmd);
	system(cmd);
	// todo : add gateway
	memset(cmd,0,OTT_BUF_SIZE);
	sprintf(cmd,"setprop bridge.eth0.gateway %s",ott_iface_gw);
	OTT_DEBUG("shell # %s\n",cmd);
	system(cmd);
	return 0;
}

int dhcp_set_datetime(char *strtime)
{
	// date -s YYYYMMDD.hhmmss
	if (!strtime || !strcmp("00000000.000000",strtime)) return 0;
	char cmd[OTT_BUF_SIZE];
	memset(cmd,0,sizeof(cmd));
	sprintf(cmd,"date -s \"%s\"",strtime);
	OTT_DEBUG("shell # %s\n",cmd);
	system(cmd);
	return 0;
}

int dhcp_check_netlink(char *ifname)
{
	OTT_DEBUG("ottdhcp check netlink:\n");
	if (!ifname) return 0;
	int len = OTT_BUF_SIZE*20;
	char buf[OTT_BUF_SIZE*20];
	int fd = socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
	if (fd < 1) {
		OTT_DEBUG("open socket error\n");
		return 0;
	}
	setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &len, sizeof(len));
	struct sockaddr_nl addr;
	memset(&addr, 0, sizeof(addr));
	addr.nl_family = AF_NETLINK;
	addr.nl_groups = RTNLGRP_LINK;
	int ret = bind(fd, (struct sockaddr*)&addr,sizeof(addr));
	if (ret < 0){
		OTT_DEBUG("bind socket error\n");
		close(fd);
		return 0;
	}
	ott_sock_netlink = fd;

	struct nlmsghdr *nh;
	struct ifinfomsg *ifinfo;
	int ifup = 0;
	int hasdown = 0;
	while((ret = read(fd, buf, sizeof(buf))) > 0){
		ifup = 0;
		for (nh=(struct nlmsghdr*)buf; NLMSG_OK(nh, ret); nh=NLMSG_NEXT(nh, ret)){
			if (nh->nlmsg_type == NLMSG_DONE) break;
			if (nh->nlmsg_type == NLMSG_ERROR) return 0;
			if (nh->nlmsg_type != RTM_NEWLINK) continue;
			// new netlink event
			ifinfo = NLMSG_DATA(nh);
			OTT_DEBUG("%u : %s\n",ifinfo->ifi_index,
					(ifinfo->ifi_flags & IFF_LOWER_UP) ? "up" : "down");
			if (if_nametoindex(ifname) != ifinfo->ifi_index) continue;	//not ifname
			if (ifinfo->ifi_flags & IFF_LOWER_UP) ifup = 1;				//ifname up
			else hasdown = 1;											//has down
		}
		if (ifup && hasdown) break;
	}
	if (ott_sock_netlink != -1) close(ott_sock_netlink);
	ott_sock_netlink = -1;
	return 0;
}

int dhcp_request_cmd(char *ifname, uint8_t cmd)
{
	OTT_DEBUG("ottdhcp request cmd %d\n",cmd);
	if (!ifname) return 0;
	uint8_t broadcast[] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
	char rawbuf[OTT_BUF_SIZE];
	size_t len = 0;
	struct sysinfo uptime;
	long begin, current;
	int found = 0;
	while(1){
		// send broadcast to check if is bridge mode
		sysinfo(&uptime);
		begin = current = uptime.uptime;
		len = dhcp_build_rawbuf(rawbuf,sizeof(rawbuf),
				ott_self_mac,broadcast,0,cmd,NULL,0);
		dhcp_sendto(ott_sock_rawfd,ifname,rawbuf,len);
		// recv packet and filter 0x7777
		while(current-begin < OTT_REQ_TIMEOUT){
			len = dhcp_recvfrom(ott_sock_rawfd,ifname,rawbuf,OTT_BUF_SIZE);
			found = dhcp_check_packet(rawbuf,len,cmd);
			if (found) break;
			sysinfo(&uptime);
			current = uptime.uptime;
		}
		if (!found) continue;		// not found packet, resend
		OTT_DEBUG("ottdhcp receive packet\n");
		dhcp_dump_raw(rawbuf);
		dhcp_parser_packet(rawbuf,len);
		// send recv commit to gateway
		len = dhcp_build_rawbuf(rawbuf,sizeof(rawbuf),
				ott_self_mac,ott_gateway_mac,1,cmd,NULL,0);
		dhcp_sendto(ott_sock_rawfd,ifname,rawbuf,len);
		switch (cmd){
			case 0: // discover gateway
				break;
			case 1: // check ip mode, 0:route, 1:bridge
				if (ott_bridge_mode == 1)
					dhcp_request_cmd(ifname,2);
				else
					system("setprop bridge.eth0.gateway \"\"");
				break;
			case 2: // request ip
				dhcp_set_network(ifname);
				break;
			case 3: // get gateway time
				dhcp_set_datetime(ott_gateway_time);
				break;
		}
		break;
	}
	return 0;
}

int dhcp_core(char *ifname)
{
	OTT_DEBUG("run as ottdhcp client\n");
	if (!ifname) return 0;
	do{
		ott_sock_rawfd = dhcp_init_rawfd(ifname, ott_self_mac, &ott_iface_addr);
		if (ott_sock_rawfd < 1) {
			sleep(1);
			continue;
		}
		ott_bridge_mode = 0;
		memset(ott_gateway_mac,0,sizeof(ott_gateway_mac));
		memset(ott_gateway_time,0,sizeof(ott_gateway_time));
		memset(ott_iface_ip,0,sizeof(ott_iface_ip));
		memset(ott_iface_gw,0,sizeof(ott_iface_gw));
		memset(ott_iface_mask,0,sizeof(ott_iface_mask));
	
		dhcp_request_cmd(ifname,0);		// find gateway
		dhcp_request_cmd(ifname,1);		// check ip mode: bridge, route
		dhcp_request_cmd(ifname,3);		// get gateway datetime
		
		if (ott_sock_rawfd!=-1) close(ott_sock_rawfd);
		ott_sock_rawfd = -1;

		// listen netlink : ethernet up event
		dhcp_check_netlink(ifname);
	}while(1);
	return 0;
}

// ---------------- ottdhcpd server ---------------

int dhcpd_parser_cmd(char *rawbuf, size_t buflen, uint8_t *clientmac)
{
	uint8_t broadcast[] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
	if (!rawbuf || buflen<18 || !clientmac) return -1;
	uint8_t *recvmac = (uint8_t*)rawbuf;
	if (!dhcp_mac_equal(ott_self_mac,recvmac,6) &&
			!dhcp_mac_equal(broadcast,recvmac,6))
		return -1;

	int offset = 18;		// mac6+6, ether_type 2, len 2, type 1, cmd 1
	if ((int)buflen < offset) return 0;
	uint16_t ethertype = (rawbuf[12]<<8) | (rawbuf[13]);
	if (ethertype != OTT_ETH_TYPE) return -1;
	uint8_t type = (uint8_t) rawbuf[16];
	if (type==1 || type!=0) return -1;
	uint8_t cmd = (uint8_t) rawbuf[17];

	// get client mac
	memset(clientmac,0,6);
	memcpy(clientmac,rawbuf+6,6);
	// data length
	uint16_t len = (rawbuf[14]<<8) | (rawbuf[15]);
	rawbuf += offset;

	return cmd;
}

int dhcpd_core(char *ifname)
{
	OTT_DEBUG("run as ottdhcpd server\n");
	if (!ifname) return 0;
	uint8_t clientmac[6];
	ott_sock_rawfd = dhcp_init_rawfd(ifname, ott_self_mac, &ott_iface_addr);
	
	char rawbuf[OTT_BUF_SIZE];
	char pkdata[OTT_BUF_SIZE];
	size_t len = 0;
	int cmd = 0;
	while(1){
		len = dhcp_recvfrom(ott_sock_rawfd,ifname,rawbuf,sizeof(rawbuf));
		if (len<1) continue;		// recvfrom timeout, redo
		cmd = dhcpd_parser_cmd(rawbuf,len,clientmac);
		if (cmd == -1) continue;	// error cmd or error packet
		switch (cmd){
			case 0: // request discover gateway
				OTT_DEBUG("ottdhcpd receive discover\n");
				dhcp_dump_raw(rawbuf);
				memset(pkdata,0,sizeof(pkdata));
				break;
			case 1: // request ip mode
				OTT_DEBUG("ottdhcpd receive ip mode request\n");
				dhcp_dump_raw(rawbuf);
				memset(pkdata,0,sizeof(pkdata));
				sprintf(pkdata,"1");		// 1:bridge, 0:route
				break;
			case 2: // request ip
				OTT_DEBUG("ottdhcpd receive ip request\n");
				dhcp_dump_raw(rawbuf);
				memset(pkdata,0,sizeof(pkdata));
				sprintf(pkdata,"ottip=%s,netmask=%s,gw=%s",
						"192.168.1.21","255.255.255.0","192.168.1.1");
				break;
			case 3: // request datetime
				OTT_DEBUG("ottdhcpd receive datetime request\n");
				dhcp_dump_raw(rawbuf);
				memset(pkdata,0,sizeof(pkdata));
				sprintf(pkdata,"20180711140000");
				break;
		}
		len = dhcp_build_rawbuf(rawbuf,sizeof(rawbuf),
					ott_self_mac,clientmac,1,cmd,pkdata,strlen(pkdata));
		dhcp_sendto(ott_sock_rawfd,ifname,rawbuf,len);
	}
	if (ott_sock_rawfd!=-1) close(ott_sock_rawfd);
	return 0;
}

int main (int argc, char *argv[])
{
	signal(SIGINT,destroy);
	char *ifname = OTT_IF_NAME;
	if (argc==2) ifname = argv[1];
	if (strcmp(ifname,"&")==0) ifname = OTT_IF_NAME;
	if (argc<3) {
		if (strstr(argv[0],"ottdhcpd"))
			dhcpd_core(ifname);
		else if (strstr(argv[0],"ottdhcp"))
			dhcp_core(ifname);
		else printf ("This tool must be ottdhcp or ottdhcpd\n");
	}
	else printf("param error\n");
	return 0;
}
